#ifndef _H_GLOBAL1
#define _H_GLOBAL1

#include <stdio.h>
#include <iostream>
#include <string>
#include <cstring>
#include <sys/time.h>
#include <pthread.h>
#include <vector>
#include <cmath>
#include "phidget21.h"

// Comment out to disable debug messages
#define SHOW_DEBUG
// Comment out to disable error messages
#define SHOW_ERROR

// Sensor type mapping for interface board
#ifndef SENSOR_SLOT_0
	#define SENSOR_SLOT_0 Sensor
#endif
#ifndef SENSOR_SLOT_1
	#define SENSOR_SLOT_1 Sensor
#endif
#ifndef SENSOR_SLOT_2
	#define SENSOR_SLOT_2 Sensor
#endif
#ifndef SENSOR_SLOT_3
	#define SENSOR_SLOT_3 Sensor
#endif
#ifndef SENSOR_SLOT_4
	#define SENSOR_SLOT_4 Sensor
#endif
#ifndef SENSOR_SLOT_5
	#define SENSOR_SLOT_5 Sensor
#endif
#ifndef SENSOR_SLOT_6
	#define SENSOR_SLOT_6 Sensor
#endif
#ifndef SENSOR_SLOT_7
	#define SENSOR_SLOT_7 Sensor
#endif

#define SONAR_CONV_RATIO_RAW 316LL/1000LL

using namespace std;

// Device class, all devices inherit this class
class Device
{
	public:
		 // Close connection
		virtual void Close() {};
		// Open connection
		virtual int Open(int SerialNumber) { return 0;};
	protected:
		static int AttachHandler(CPhidgetHandle Handle, void *UserPtr); // Attach handler
		static int DetachHandler(CPhidgetHandle Handle, void *UserPtr); // Detach handler
		static int ErrorHandler(CPhidgetHandle Handle, void *UserPtr, int ErrorCode, const char *Description); // Error handler
		int wait(unsigned long milisec);
};

// Sensor types for interface board
class Sensor // Generic sensor (no value conversion)
{
	public:
		virtual void Update(int Index, int Value);
		virtual int GetValue();
		Sensor(CPhidgetInterfaceKitHandle boardHandle);
		~Sensor();
		void (*UpdateHandle)(int Vlaue, int PrevValue);
	protected:
		int value;
		int _Index;
		CPhidgetInterfaceKitHandle BoardHandle;
	private:
		
};

struct SensorTemp
{
	Sensor* ptr;
	int Value;
	int Index;
};

class Sonar : public Sensor
{
	public:
		Sonar(CPhidgetInterfaceKitHandle boardHandle);
		void Update(int Index, int Value);
	protected:
	private:
};

class IR : public Sensor
{
	public:
		IR(CPhidgetInterfaceKitHandle boardHandle);
		void Update(int Index, int Value);
	protected:
	private:
};

// Interface board class
class InterfaceBoard : public Device
{
	public:
		InterfaceBoard(); // Constructor
		~InterfaceBoard(); // Destructor
		int Open(int SerialNumber); // Open new connection
		void Close(); // Close connection
		int GetSensorValue(unsigned int Index); // Get analog sensor value
		int GetInputState(unsigned int Index); // Get digital input state
		int GetOutputState(unsigned int Index); // Get digital input state
		void SetOutputState(unsigned int Index, int State); // Set digital output state
		void UpdateSensors(); // Forces sensors to update their values
		Sensor* Sensors[8]; // Sensor values
	protected:
	private:
		CPhidgetInterfaceKitHandle Handle; // Device handle
		
		int InputVals[8]; // Input values
		int OutputVals[8]; // Input values
		
		static int InputChangeHandler(CPhidgetInterfaceKitHandle hanlde, void *UserPtr, int Index, int State); // Input change handler
		static int OutputChangeHandler(CPhidgetInterfaceKitHandle handle, void *UserPtr, int Index, int State); // Output change handler
		static int SensorChangeHandler(CPhidgetInterfaceKitHandle handle, void *UserPtr, int Index, int Value); // Sensor change handler
};

// Encoder board
class Encoder : public Device
{
	public:
		void Close(); // Close connection
		int Open(int SerialNumber);
		int GetValue();
		void Reset();
		Encoder();
		~Encoder();
	protected:
		int Position;
	private:
		static int InputChangeHandler(CPhidgetEncoderHandle handle, void *UserPtr, int Index, int State);
		static int PositionChangeHandler(CPhidgetEncoderHandle handle, void *UserPtr, int Index, int Time, int RelativePosition);
		CPhidgetEncoderHandle Handle;
};

// DC motor control board
class DCMotor : public Device
{
	public:
		void Close(); // Close connection
		int Open(int SerialNumber);
		void SetSpeed(int MotorIndex, int Speed, int Acceleration);
		int GetInputState(unsigned int Index); // Get digital input state
		DCMotor();
		~DCMotor();
	private:
		static int InputChangeHandler(CPhidgetMotorControlHandle handle, void *UserPtr, int Index, int State);
		static int VelocityChangeHandler(CPhidgetMotorControlHandle handle, void *UserPtr, int Index, double Value);
		static int CurrentChangeHandler(CPhidgetMotorControlHandle handle, void *UserPtr, int Index, double Value);
		CPhidgetMotorControlHandle Handle;
		int InputVals[4];
};

class Servo : public Device
{
	public:
		void Close(); // Close connection
		int Open(int SerialNumber);
		void SetPosition(int Position);
		int GetPosition();
		Servo();
		~Servo();
	private:
		int Position;
		static int PositionChangeHandler(CPhidgetAdvancedServoHandle handle, void *UserPtr, int Index, double Value);
		CPhidgetAdvancedServoHandle Handle;
};

class Manager
{
	public:
		void Close(); // Close connection
		int Open();
		void SetPosition(int Position);
		void GetDevices(CPhidgetHandle* deviceHandles, int* Count);
		void GetDeviceDescription(CPhidgetHandle handle, CPhidget_DeviceClass* Class, CPhidget_DeviceID* ID, int* SerialNumber, string& Type);
		Manager();
		~Manager();
	private:
		CPhidgetHandle DeviceHandles[32];
		int DeviceCount;
		static int AttachHandler(CPhidgetHandle Handle, void *UserPtr);
		static int DetachHandler(CPhidgetHandle Handle, void *UserPtr);
		static int ErrorHandler(CPhidgetManagerHandle Handle, void *UserPtr, int Code, const char *Description);
		void UpdateDeviceList();
		CPhidgetManagerHandle Handle;
};

#endif
